// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PicknPlaceFSMContext.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Microsoft.Robotics.Manipulation.PicknPlace
{
    using System;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.FiniteStateMachine;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.PhysicalObjects;
    using Microsoft.Robotics.Runtime;

    /// <summary>
    /// Context for a Pick or place FSM
    /// </summary>
    public class PicknPlaceFSMContext : IContext<PicknPlaceFSMContext, PicknPlaceFSMContext>
    {
        /// <summary>
        /// Threshold for determining if gripper is in open position. Expressed as a percentage of the delta between current and desired
        /// </summary>
        private readonly double gripperOpenThresholdDelta = 5;

        /// <summary>
        /// Threshold for determining if gripper is closed. Expressed as a percentage of the delta between current and desired
        /// </summary>
        private readonly double gripperCloseThresholdDelta = 10;

        /// <summary>
        /// Captures the start time of a sub task
        /// </summary>
        private DateTime startSubTaskTime;

        /// <summary>
        /// Initializes a new instance of the <see cref="PicknPlaceFSMContext" /> class
        /// </summary>
        /// <param name="armOrientationTolerance">Tolerance on arm moves - orientation</param>
        /// <param name="armPositionTolerance">Tolerance on arm moves - position</param>
        /// <param name="manipulatedObjectDescription">Picked object</param>
        /// <param name="pickPlacePose">Object pose in pick or place location </param>
        /// <param name="retractPose">Retraction pose</param>
        /// <param name="lastRequestMessage">Incoming pick or place message triggering the pick or place operation</param>
        public PicknPlaceFSMContext(
            double armOrientationTolerance, 
            double armPositionTolerance,
            PhysicalObjectDescription manipulatedObjectDescription,
            Pose pickPlacePose,
            Pose retractPose,
            AgentMessage lastRequestMessage)
        {
            this.ArmOrientationTolerance = armOrientationTolerance;
            this.ArmPositionTolerance = armPositionTolerance;
            this.ManipulatedObjectDescription = manipulatedObjectDescription;
            this.PickPlacePose = pickPlacePose;
            this.RetractPose = retractPose;
            this.LastRequestMessage = lastRequestMessage;
        }

        /// <summary>
        /// Gets or sets the target parameters for the current move and grasp task
        /// </summary>
        public MoveAndGraspTaskDescription TargetTask { get; set; }

        /// <summary>
        /// Gets or sets state of the hand
        /// </summary>
        public GripperStatus CurrentHandState { get; set; }

        /// <summary>
        /// Gets or sets the current arm pose
        /// </summary>
        public Pose CurrentArmPose { get; set; }

        /// <summary>
        /// Gets the message that initiated the last pick or place operation 
        /// </summary>
        public AgentMessage LastRequestMessage { get; private set; }

        /// <summary>
        /// Gets or sets the last target arm pose. Null if there is no
        /// previous target. This was added as a workaround to detecting dropped move messages 
        /// when working with the IK arm controller
        /// </summary>
        public Pose PreviousArmTargetPose { get; set; }

        /// <summary>
        /// Gets a description of the picked object
        /// </summary>
        [DataMember]
        public PhysicalObjectDescription ManipulatedObjectDescription { get; private set; }

        /// <summary>
        /// Gets object's pick or place location and orientation
        /// </summary>
        [DataMember]
        public Pose PickPlacePose { get; private set; }

        /// <summary>
        /// Gets the final retraction location and orientation
        /// </summary>
        [DataMember]
        public Pose RetractPose { get; private set; }

        /// <summary>
        /// Gets or sets inputs to the FSM from the outside world.
        /// </summary>
        public PicknPlaceFSMContext Input { get; set; }

        /// <summary>
        /// Gets or sets inputs to the FSM from the outside world.
        /// </summary>
        public PicknPlaceFSMContext Output { get; set; }

        /// <summary>
        /// Gets the tolerance for arm orientation move convergence
        /// </summary>
        public double ArmOrientationTolerance { get; private set; }

        /// <summary>
        /// Gets the tolerance for arm position move convergence
        /// </summary>
        public double ArmPositionTolerance { get; private set; }

        /// <summary>
        /// Gets or sets the start move pose
        /// </summary>
        public Pose StartMovePose { get; set; }

        /// <summary>
        /// Sets the start time of sub task
        /// </summary>
        public void StartSubTaskTimer()
        {
            this.startSubTaskTime = DateTime.Now;
        }

        /// <summary>
        /// Sets the start time of an arbitrary sub task
        /// </summary>
        /// <param name="startSubTaskTime">Sub task start time</param>
        public void StartSubTaskTimer(out DateTime startSubTaskTime)
        {
            startSubTaskTime = DateTime.Now;
        }

        /// <summary>
        /// Determines if a subtask has timed out
        /// </summary>
        /// <param name="delayInMSec">Time out wait in mSec</param>
        /// <returns>True if delay since sub task exceeds delay</returns>
        public bool IsSubTaskTimeOut(int delayInMSec)
        {
            return this.IsSubTaskTimeOut(delayInMSec, this.startSubTaskTime);
        }

        /// <summary>
        /// Determines if a subtask has timed out
        /// </summary>
        /// <param name="delayInMSec">Time out wait in mSec</param>
        /// <param name="startSubTaskTime">Delay start time</param>
        /// <returns>True if delay since sub task exceeds delay</returns>
        public bool IsSubTaskTimeOut(int delayInMSec, DateTime startSubTaskTime)
        {
            TimeSpan diff = DateTime.Now - startSubTaskTime;
            return diff.TotalMilliseconds > delayInMSec;
        }

        /// <summary>
        /// Decides if hand is in desired state
        /// </summary>
        /// <returns>True if hand is in target state</returns>
        public bool IsHandReady()
        {
            bool ready = false;
            if (null == this.CurrentHandState)
            {
                return ready;
            }

            double delta = Math.Abs(this.TargetTask.GripperState.PercentOpen - this.CurrentHandState.PercentOpen);

            if (this.TargetTask.GripperState.GripStateEnum == GripperStatus.GripperStateEnum.Open)
            {
                ready = (delta < this.gripperOpenThresholdDelta);
            }
            else if (this.TargetTask.GripperState.GripStateEnum == GripperStatus.GripperStateEnum.Close)
            {
                ready = (delta < this.gripperCloseThresholdDelta);
            }

            return ready;
        }

        /// <summary>
        /// Decides if an arm move action is complete
        /// </summary>
        /// <returns>True if the move is complete</returns>
        public bool IsArmMoveComplete()
        {
            return this.IsArmMoveComplete(this.TargetTask.ArmPose);
        }

        /// <summary>
        /// Decides if an arm move action is complete
        /// </summary>
        /// <param name="targetArmPose">Target pose at completion</param>
        /// <returns>True if the move is complete</returns>
        public bool IsArmMoveComplete(Pose targetArmPose)
        {
            double quatDiff = Quaternion.Length(this.CurrentArmPose.Orientation - targetArmPose.Orientation);
            double posDiff = this.CurrentArmPose.Position.DistanceFrom(targetArmPose.Position);
            bool ready = quatDiff < this.ArmOrientationTolerance &&
                posDiff < this.ArmPositionTolerance;

            if (quatDiff > this.ArmOrientationTolerance && Math.Abs(targetArmPose.Orientation.W) < 0.001)
            {
                // Corner case check for equivalent rotations for ~180' rotations  (w small)
                double equivThreshold = this.ArmOrientationTolerance / 2.0;
                if (Math.Abs(Math.Abs(targetArmPose.Orientation.X) - Math.Abs(this.CurrentArmPose.Orientation.X)) < equivThreshold &&
                    Math.Abs(Math.Abs(targetArmPose.Orientation.Y) - Math.Abs(this.CurrentArmPose.Orientation.Y)) < equivThreshold &&
                    Math.Abs(Math.Abs(targetArmPose.Orientation.Z) - Math.Abs(this.CurrentArmPose.Orientation.Z)) < equivThreshold &&
                    Math.Abs(Math.Abs(targetArmPose.Orientation.W) - Math.Abs(this.CurrentArmPose.Orientation.W)) < equivThreshold)
                {
                    ready = posDiff < this.ArmPositionTolerance;
                }
            }

            return ready;
        }
    }
}
